#ifndef __DRV_DM_H
#define __DRV_DM_H
#include "main.h"
#include "bsp_can.h"

/* ----------   达妙电机   ---------- */
#define DM_MODE_POS                 2
#define DM_MODE_POS_OFFSET          0x100
#define DM_MODE_SPD                 3
#define DM_MODE_SPD_OFFSET          0x200
#define DM_MODE_MIT                 1
#define DM_MODE_MIT_OFFSET          0x00

#define DM_INS_WRITE                0x55      // 写入寄存器的指令
#define DM_INS_SAVE                 0xAA      // 保存寄存器的指令
#define DM_STDID_BROAD              0x7FF
#define DM_RID_CTRL_MODE            10
#define DM_RID_PMAX                 21
#define DM_RID_VMAX                 22
#define DM_RID_CAN_BR               35
#define DM_CAN_BR_1M                4
#define DM_POS_FLOAT_MAX            100.0f
#define DM_POS_FLOAT_MIN            -100.0f
#define DM_VEL_FLOAT_MAX            100.0f
#define DM_VEL_FLOAT_MIN            -100
#define DM_TOR_FLOAT_MAX            3.0f
#define DM_TOR_FLOAT_MIN            -3.0f
#define DM_KP_MIN 0.0f
#define DM_KP_MAX 500.0f
#define DM_KD_MIN 0.0f
#define DM_KD_MAX 5.0f

typedef enum {
  mit_mode = 1,
  pos_mode = 2,
  spd_mode = 3
} dm_mode_enum;

typedef enum {
  mit_mode_offset = 0x000,
  pos_mode_offset = 0x100,
  spd_mode_offset = 0x200
} dm_mode_offset_enum;

typedef struct 
{
  /* ---------- 基本信息 ---------- */
  uint16_t id;            // 电机ID
  uint16_t mst_id;        // 主控ID
  float pos;              // 当前位置，单位：度
  float pos_max;          // 最大位置，单位：度
  float vel_max;          // 最大速度，单位：RPM
  float tor_max;          // 最大扭矩，单位：N·m
  /* ---------- 控制信息 ---------- */
  dm_mode_enum mode;                  // 模式：1.MIT  2.位置速度  3.速度  4.力位混控
  dm_mode_offset_enum mode_offset;    // 模式偏移：通过(mode_offset + id)选中电机
  uint8_t read_lock;                  // 读取标志，0：空闲，1：读锁使能
  uint8_t write_lock;                 // 写入标志，0：空闲，1：写锁使能
} motor_dm_t;

typedef struct {
  float p_des;    // 目标位置 (rad)
  float v_des;    // 目标速度 (rad/s) 
  float kp;       // 位置比例系数 [0,500]
  float kd;       // 位置微分系数 [0,5]
  float t_ff;     // 前馈转矩 (Nm)
} dm_mit_ctrl_t;



void DM_Init(motor_dm_t* dm_ptr, uint16_t id, uint16_t mst_id, dm_mode_enum mode);
uint8_t DM_Enable(motor_dm_t* dm_ptr);
uint8_t DM_Disable(motor_dm_t* dm_ptr);
uint8_t DM_ResetZero(motor_dm_t* dm_ptr);

uint8_t DM_DriveSpeedCtrl(CAN_HandleTypeDef* hcan, uint16_t motor_id, float vel);
uint8_t DM_DriveMITCtrl (CAN_HandleTypeDef* hcan, uint16_t motor_id, 
                      float pos, float vel,float kp, float kd, float tor);
uint8_t DM_DrivePOSCtrl(CAN_HandleTypeDef* hcan, uint16_t motor_id, float pos, float vel);
uint8_t DM_WriteReg(uint16_t raw_id, uint8_t rid, uint8_t* d);
uint8_t DM_SaveReg(uint16_t id, uint8_t rid);
uint8_t DM_GotoZero(motor_dm_t* dm_ptr);
uint8_t DM_RequestFB(motor_dm_t* dm_ptr);
int float_to_uint(float x_float, float x_min, float x_max, int bits);
float uint_to_float(int x_int, float x_min, float x_max, int bits);
#endif
